#include "Controller.h"

// ==================================================================
MPC_Formulation::MPC_Formulation(){
}
// ------------------------------------------------------------------
MPC_Formulation::MPC_Formulation(int i_num_MV, int i_num_para){
  num_MV = i_num_MV;
  num_para = i_num_para;
}
// ------------------------------------------------------------------
MPC_Formulation::~MPC_Formulation(){
} 
// ==================================================================




// ==================================================================
controller_MPC_IPOPT::controller_MPC_IPOPT(){
}
// ------------------------------------------------------------------
controller_MPC_IPOPT::controller_MPC_IPOPT(int i_num_input, int iMode, 
					   int i_num_output,
					   double idt)
{
  int i;
  
  // I/O parameters
  mode = iMode;
  int* i_dim = new int[i_num_input];
  for(i=0;i<2;i++){
    i_dim[i] = (iMode+1);
  }
  for(i=2;i<i_num_input;i++){
    i_dim[i] = 1;
  }
  int* o_dim = new int[i_num_output];
  for(i=0;i<i_num_output;i++){
    o_dim[i]= 1;
  }
  block::initialize(i_num_input,i_dim,i_num_output,o_dim,idt);
  delete []i_dim;
  delete []o_dim;

  // MPC parameters 
  problem  = NULL;
  problem_base = NULL;
  solver = NULL;
  solution = new double[i_num_output];

  reset();
}
// ------------------------------------------------------------------
controller_MPC_IPOPT::~controller_MPC_IPOPT(){
  delete []solution;
}
// ------------------------------------------------------------------
void controller_MPC_IPOPT::reset(){
  time = 0.0;
  if(problem!=NULL){
    problem->reset();
    problem->get_solution(solution);
    for(int i=0;i<block::output_num;i++){
	outputs[i][0] = solution[i];
    }
  } 
}
// ------------------------------------------------------------------
int controller_MPC_IPOPT::load_formulation(MPC_Formulation* iProblem){
  if(problem != NULL){
    cout << "Already have a MPC formulation\n Return\n";
    return 0;
  }
  assert(block::output_num==iProblem->get_num_MV());
  problem = iProblem;
  problem_base = problem;
  solver = IpoptApplicationFactory();
  problem->reset();
  
  solver->Options()->SetStringValue("hessian_approximation","limited-memory");
  //	solver->Options()->SetStringValue("derivative_test","none");
  solver->Options()->SetStringValue("derivative_test","first-order");
  status = solver->Initialize();
  if (status != Solve_Succeeded){
    printf("\n\n*** Error during initialization!\n");
    //return (int) status;
  }
  return 0;
}
// ------------------------------------------------------------------
/*int controller_MPC_IPOPT::setSP(int sp_id,double sp_val){
  assert(sp_id<num_sp);
  sp[sp_id] = sp_val;
  return 0;
}
// ------------------------------------------------------------------
int controller_MPC_IPOPT::setSP(double* sp_vals){
  int i;
  for(i=0;i<num_sp;i++){
    sp[i] = sp_vals[i];
  }
  return 0;
  }*/
// ------------------------------------------------------------------
void controller_MPC_IPOPT::update(double sysTime){
  if(sysTime<time){
    cerr << "Error@controller_MPC_IPOPT::update(), time = "<<time << ", sysTime=" << sysTime << "\n";
    exit(1);
  }
  // Update states (no state for controller)
  while(time+dt<=sysTime){
    time = time+dt;
  }
  // Output
  problem->parameterize(time,inputs);
  status = solver->OptimizeTNLP(problem_base);
  if(status == Solve_Succeeded){
    // Index iter_count = solver->Statistics()->IterationCount();
    // printf("\n\n*** The problem solved in %d iterations!\n", iter_count);

    // Number final_obj = solver->Statistics()->FinalObjective();
    // printf("\n\n*** The final value of the objective function is %e.\n", final_obj);

    problem->get_solution(solution);
    for(int i=0;i<block::output_num;i++){
      outputs[i][0] = solution[i];
    }
  }		
}
// ------------------------------------------------------------------
